/* Copyright 2012 Dustin Mitchell <dmmitche [at] gmail [dot] com>
 *
 * Licensed under the GPLv3.  See included LICENSE.txt for full details.
 */

#include <stdint.h>
#include <signal.h>
#include <unistd.h>

#include "service.h"
#include "nmea_replay.h"
#include "sensor_service.h"
#include "message_queue.h"
#include "message_handler.h"
#include "messages.h"
#include "clock.h"
#include "beaglebot_sensors.h"
#include "sensor.h"
#include "gps_sensor.h"
#include "log.h"

volatile int run = 1;

void sig_handler (int sig)
{
  run = 0;
}

int main ()
{
  struct service nmea_svc;
  struct nmea_replay_args nmea_args;
  struct service sensor_svc;
  struct sensor_svc_args sensor_args;
  struct message_queue msg_queue;
  struct message_handler msg_handler;
  union message quit_msg;
  union sensor sensors[BEAGLEBOT_NUM_SENSORS];
  int i;

  if (clock_init ()) {
    log (LOG_ERR, "unable to init clock system");
    return 1;
  }

  if (message_queue_create (&msg_queue) == -1) {
    log (LOG_ERR, "unable to create message queue\n");
    return 1;
  }

  if (message_handler_create (&msg_handler) == -1) {
    log (LOG_ERR, "unable to create message handler");
    return 1;
  }

  if (message_queue_register_handler (&msg_queue, &msg_handler, SENSOR_MESSAGE)
      == -1) {
    log (LOG_ERR, "unable to register handler with queue");
    return 1;
  }

  for (i = 0; i < BEAGLEBOT_NUM_SENSORS; ++i) {
    if (sensor_create (&sensors[i], &BEAGLEBOT_SENSOR_DEFS[i]) == -1) {
      log (LOG_ERR, "unable to create sensor %d", i);
      return 1;
    }
  }

  nmea_args.uart = gps_sensor_get_uart (&sensors[BEAGLEBOT_GPS_SENSOR].gps);
  nmea_args.nmea_file = "nmea_sample.log";

  if (service_start (&nmea_svc, &msg_queue, nmea_replay_service, &nmea_args)
      == -1) {
    log (LOG_ERR, "unable to start NMEA replay service");
    return 1;
  }

  sensor_args.sensors = sensors;
  sensor_args.n_sensors = BEAGLEBOT_NUM_SENSORS;

  if (service_start (&sensor_svc, &msg_queue, sensor_service, &sensor_args)
      == -1) {
    log (LOG_ERR, "unable to start sensor service");
    return 1;
  }

  signal (SIGINT, sig_handler);

  while (run) {
    union message msg;

    if (message_handler_wait (&msg_handler, &msg) == -1) {
      log (LOG_ERR, "unable to wait on message");
      break;
    }

    switch (msg.header.type) {
      case SENSOR_MESSAGE: {
        log (LOG_INFO, "sensor id: %d", msg.sensor.sensor_id);

        switch (msg.sensor.reading.type) {
          case SENSOR_GPS:
            log (LOG_INFO, "GPS sensor reading");
            break;

          case SENSOR_WAKE_UP:
            log (LOG_INFO, "Wake up sensr reading");
            break;
        }

        break;
      }
    }
  }

  quit_msg.header.type = QUIT_MESSAGE;

  log (LOG_INFO, "sending quit message");
  message_queue_post (&msg_queue, &quit_msg);

  service_wait (&nmea_svc);

  return 0;
}
